#ifndef __KALMAN_H__
#define __KALMAN_H__

#include "main.h"

typedef struct 
{
    float x;
    float K;
    float DT;
    double estimate;           // 状态估计（角度）
    double error_cov;          // 估计误差协方差
    double process_noise;      // 过程噪声（陀螺仪信任度）
    double measure_noise;      // 测量噪声（加速度计信任度）
    double last_prediction;    // 上一次的预测值（用于调试）
} Kalman_Typedef;

// 初始化Kalman滤波器
void Kalman_Init(Kalman_Typedef *k, double initial_estimate, double initial_error_cov);

// 预测步 - 使用陀螺仪积分进行角度预测
void Kalman_predict(Kalman_Typedef *kf, double gyro_integrated_angle);

// 更新步 - 使用加速度计测量角度进行融合
float Kalman_update(Kalman_Typedef *kf, double accelerometer_angle);

// 设置噪声参数（可选，便于调试调整）
void Kalman_Set_Noise(Kalman_Typedef *kf, double process_noise, double measure_noise);

#endif